clear all; close all; clc

data = load('20110302-1.txt');

sT = 1/40;

phi = data(:,1);
phi_hat = data(:,2);
phi_dot = data(:,3);

theta = data(:,4);
theta_hat = data(:,5);
theta_dot = data(:,6);

psi = data(:,7);
psi_hat = data(:,8);
psi_dot = data(:,9);

x = data(:,10);
x_hat = data(:,11);
x_dot = data(:,12);

y = data(:,13);
y_hat = data(:,14);
y_dot = data(:,15);

z = data(:,16);
z_hat = data(:,17);
z_dot = data(:,18);

phi_d_c = data(:,19);
theta_d_c = data(:,20);
psi_d = data(:,21);

ppm1 = data(:,22);
ppm2 = data(:,23);
ppm3 = data(:,24);
ppm4 = data(:,25);

phi_d = data(:,26);
theta_d = data(:,27);

phi_d_dot = data(:,28);
theta_d_dot = data(:,29);

xd = data(:,30);
yd = data(:,31);
zd = data(:,32);

int_e_phi = data(:,33);
int_e_theta = data(:,34);
int_e_psi = data(:,35);
int_e_x = data(:,36);
int_e_y = data(:,37);
int_e_z = data(:,38);

phi_c = data(:,39);
theta_c = data(:,40);
psi_c = data(:,41);

% w_phi = data(:,28);
% w_theta = data(:,29);
% w_psi = data(:,30);

t = sT * [ 0 : length(data)-1 ];

%%
ATT = figure(1);
set(ATT,'Color','White')

PHI = subplot(3,1,1);
set(PHI,'Box','on'), hold on
plot(t,phi*180/pi), ylim([-20 20])
% plot(t,phi_c*180/pi,'r')
plot(t,phi_hat*180/pi,'r')
% plot(t,phi_d_c*180/pi,'g','linewidth',2)
% plot(t,phi_d*180/pi,'m','linewidth',2)

THETA  = subplot(3,1,2);
set(THETA,'Box','on'), hold on
plot(t,theta*180/pi), ylim([-20 20])
% plot(t,theta_c*180/pi,'r')
plot(t,theta_hat*180/pi,'r')
% plot(t,theta_d_c*180/pi,'g','linewidth',2)
% plot(t,theta_d*180/pi,'m','linewidth',2)

PSI  = subplot(3,1,3);
set(PSI,'Box','on'), hold on
plot(t,psi*180/pi), ylim([-20 20])
% plot(t,psi_c*180/pi,'r')
plot(t,psi_hat*180/pi,'r')
% plot(t,psi_d*180/pi,'g','linewidth',2)

%%
ATTDOT = figure(4);
set(ATTDOT,'Color','White')

PHIDOT = subplot(3,1,1);
set(PHIDOT,'Box','on'), hold on
plot(t,phi_dot*180/pi)

THETADOT  = subplot(3,1,2);
set(THETADOT,'Box','on'), hold on
plot(t,theta_dot*180/pi)

PSIDOT  = subplot(3,1,3);
set(PSIDOT,'Box','on'), hold on
plot(t,psi_dot*180/pi)

%%
POS = figure(2);
set(POS,'Color','White')

XX = subplot(3,1,1);
set(XX,'Box','on'), hold on
plot(t,x), ylim([-1.5 1.5])
plot(t,x_hat,'r')
% plot(t,xd,'g','linewidth',2)

YY  = subplot(3,1,2);
set(YY,'Box','on'), hold on
plot(t,y), ylim([-1.5 1.5])
plot(t,y_hat,'r')
% plot(t,yd,'g','linewidth',2)

ZZ  = subplot(3,1,3);
set(ZZ,'Box','on'), hold on
plot(t,z), ylim([-0.5 1.2])
plot(t,z_hat,'r')
plot(t,zd,'g','linewidth',2)

%%
Vel = figure(4);
set(Vel,'Color','White')

XX = subplot(3,1,1);
set(XX,'Box','on'), hold on
plot(t,x_dot), ylim([-1.5 1.5]*3)
% plot(t,x_hat,'r')
% plot(t,xd,'g','linewidth',2)

YY  = subplot(3,1,2);
set(YY,'Box','on'), hold on
plot(t,y_dot), ylim([-1.5 1.5]*3)
% plot(t,y_hat,'r')
% plot(t,yd,'g','linewidth',2)

ZZ  = subplot(3,1,3);
set(ZZ,'Box','on'), hold on
plot(t,z_dot), ylim([-1.5 1.5]*3)
% plot(t,z_hat,'r')
% plot(t,zd,'g','linewidth',2)

%%
INPUT = figure(3);
set(INPUT,'Color','White')
set(gca,'box','on')
plot(t,ppm1,t,ppm2,t,ppm3,t,ppm4)

%%
INT = figure(5);
set(INT,'Color','White')
set(gca,'box','on')

ATT_INT = subplot(2,1,1);
plot(t,int_e_phi,t,int_e_theta,t,int_e_psi)
legend('int e phi','int e theta','int e psi','location','northwest')

POS_INT = subplot(2,1,2);
plot(t,int_e_x,t,int_e_y,t,int_e_z)
legend('int e x','int e y','int e z','location','northwest')




